#include "decimate.hh"

#include <cmath>
#include <iostream>

double areaOfTriangle(const detail::Node& p0, const detail::Node& p1, const detail::Node& p2, double maxdist) {
	auto dx=p2.pos[0]-p0.pos[0];
	auto dy=p2.pos[1]-p0.pos[1];
	auto dz=p2.pos[2]-p0.pos[2];
	if(std::sqrt(dx*dx+dy*dy+dz*dz)>=maxdist)
		return INFINITY;
	auto ux=p0.pos[0]-p1.pos[0];
	auto uy=p0.pos[1]-p1.pos[1];
	auto uz=p0.pos[2]-p1.pos[2];
	auto vx=p2.pos[0]-p1.pos[0];
	auto vy=p2.pos[1]-p1.pos[1];
	auto vz=p2.pos[2]-p1.pos[2];
	auto cx=uy*vz-uz*vy;
	auto cy=uz*vx-ux*vz;
	auto cz=ux*vy-uy*vx;
	return sqrt(cx*cx+cy*cy+cz*cz)/2;
}

// Visvalingam
void detail::reducePointsImpl(std::list<detail::Node>& pts, double maxdist, double maxarea) {
	// smooth
	std::array<double, 3> prev_pos=pts.front().pos;
	for(auto it=pts.begin(); it!=pts.end(); it++) {
		if(it==pts.begin()) {
			it->a=INFINITY;
			continue;
		}
		auto it2=it;
		it2++;
		if(it2==pts.end()) {
			it->a=INFINITY;
			continue;
		}
		for(unsigned int i=0; i<3; i++) {
			auto p=it->pos[i];
			it->pos[i]=(p*6+prev_pos[i]*1+it2->pos[i]*1)/8;
			prev_pos[i]=p;
		}
		//auto r=(pts[i-1].r()*2+pts[i-2].r()*1+pts[i].r()*1)/4;
		it->a=NAN;
	}
	while(pts.size()>2) {
		double minarea=INFINITY;
		std::list<detail::Node>::iterator miniter;
		{
			auto i=pts.begin();
			i++;
			auto j=i;
			j++;
			for(; j!=pts.end(); i++, j++) {
				if(std::isnan(i->a)) {
					auto k=i;
					k--;
					i->a=areaOfTriangle(*k, *i, *j, maxdist);
				}
				if(i->a<minarea) {
					minarea=i->a;
					miniter=i;
				}
		//std::cerr<<"area: "<<i->a<<' '<<i->i<<' '<<maxdist<<"\n";
			}
		}
		if(minarea>=maxarea)
			break;
		auto prev=miniter;
		prev--;
		auto next=miniter;
		next++;
		for(unsigned int k=0; k<3; k++) {
			prev->pos[k]=(prev->pos[k]*4+miniter->pos[k])/5;
			next->pos[k]=(next->pos[k]*4+miniter->pos[k])/5;
		}
		pts.erase(miniter);
		if(prev!=pts.begin()) {
			auto pprev=prev;
			pprev--;
			prev->a=NAN;//areaOfTriangle(*pprev, *prev, *next, maxdist);
			if(pprev!=pts.begin()) {
				auto ppprev=pprev;
				ppprev--;
				pprev->a=NAN;//areaOfTriangle(*ppprev, *pprev, *prev, maxdist);
			}
		}
		auto nnext=next;
		nnext++;
		if(nnext!=pts.end()) {
			next->a=NAN;//areaOfTriangle(*prev, *next, *nnext, maxdist);
			auto nnnext=nnext;
			nnnext++;
			if(nnnext!=pts.end()) {
				nnext->a=NAN;//areaOfTriangle(*next, *nnext, *nnnext, maxdist);
			}
		}
	}
	//for(auto& pt: pts) {
		//std::cerr<<"dec: "<<pt.pos[2]<<' '<<pt.i<<'\n';
	//}
	//for(size_t i=2; i<outpts.size(); i++)
		//fprintf(stderr, "lastarea: %lf\n", areaOfTriangle(outpts[i-2], outpts[i-1], outpts[i], maxdist));
}

